#include "Bsp_MsgSend.h"
#include "Bsp_Uart.h"
#include <string.h>
#include <stdio.h>
#include "Bsp.h"

// 定义帧头和帧尾
#define FRAME_HEADER 0x55
#define FRAME_TAIL   0xBB

/**
 * @brief 发送指定格式的数据帧
 * @param command 命令码
 * @param data 数据缓冲区
 * @param length 数据长度
 */
void msg_send(uint8_t command, const uint8_t *data, uint8_t length) {
    uint8_t checksum = 0;
    uint8_t frame[260]; // 最大数据帧大小
    int index = 0;

    // 帧头
    frame[index++] = FRAME_HEADER;
    frame[index++] = FRAME_HEADER;

    // 命令码
    frame[index++] = command;
    checksum += command;

    // 数据长度
    frame[index++] = length;
    checksum += length;

    // 数据域
    for (int i = 0; i < length; ++i) {
        frame[index++] = data[i];
        checksum += data[i];
    }

    // 校验和
    frame[index++] = checksum;

    // 帧尾
    frame[index++] = FRAME_TAIL;

////     打印调试信息（可选）
//     printf("Send Frame: Command=0x%02X, Length=%d\n", command, length);
//     for (int i = 0; i < length; ++i) {
//         printf("Data[%d] = 0x%02X\n", i, data[i]);
//     }
//     printf("Checksum = 0x%02X\n", checksum);

    // 使用串口发送
    USART1_Send_ArrayU8(frame, index);
}

/**
 * @brief 发送 IMU 数据
 */
void send_imu_data(uint32_t timestamp, const float accel[3], const float gyro[3]) {
	uint8_t buffer[28]; // 4(timestamp) + 12(accel) + 12(gyro) = 28 bytes

	// 填充时间戳
	memcpy(&buffer[0], &timestamp, 4);

	// 填充加速度数据
	memcpy(&buffer[4], &accel[0], 4);
	memcpy(&buffer[8], &accel[1], 4);
	memcpy(&buffer[12], &accel[2], 4);

	// 填充陀螺仪数据
	memcpy(&buffer[16], &gyro[0], 4);
	memcpy(&buffer[20], &gyro[1], 4);
	memcpy(&buffer[24], &gyro[2], 4);

    msg_send(CMD_IMU_DATA, buffer, 28);
}

/**
 * @brief 发送小车三轴速度数据
 */
void send_speed_data(int16_t vx, int16_t vy, int16_t vz) {
    uint8_t data[6];

    data[0] = (vx >> 8) & 0xFF;
    data[1] = vx & 0xFF;
    data[2] = (vy >> 8) & 0xFF;
    data[3] = vy & 0xFF;
    data[4] = (vz >> 8) & 0xFF;
    data[5] = vz & 0xFF;

    msg_send(CMD_SPEED_DATA, data, 6);
}

/**
 * @brief 发送电池电压数据
 * @param voltage 电池电压（例如：单位为 mV，则 12.3V 表示为 12300）
 */
void send_battery_voltage(uint16_t voltage) {
    uint8_t data[2];

    data[0] = (voltage >> 8) & 0xFF; // 高字节
    data[1] = voltage & 0xFF;        // 低字节

    printf("Sending Battery Voltage: %dmV\n", voltage); // 单位假设为 mV
    msg_send(CMD_BATTERY, data, 2);
}
